#pragma config(Sensor, dgtl1,  ArmEncoderLeft,      sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  ArmEncoderRight,     sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  BaseEncoderLeft,     sensorRotation)
#pragma config(Sensor, dgtl6,  BaseEncoderRight,    sensorRotation)
#pragma config(Sensor, dgtl7,  ProgramSwitch,       sensorTouch)
#pragma config(Motor,  port1,           LeftBase,      tmotorNormal, openLoop)
#pragma config(Motor,  port4,           LeftArm,       tmotorNormal, openLoop)
#pragma config(Motor,  port5,           RightArm,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           LeftVacume,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           RightVacume,   tmotorNormal, openLoop)
#pragma config(Motor,  port8,           LeftTurntable, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port9,           RightTurntable, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          RightBase,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)


//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!


// Start Autonomous Voids
const int SquareAndThreeQuarters = 38;
const int SquareAndAHalf = 35;
const int Square = 35;
int motorpowerright = 0;
int motorpowerleft = 0;
bool VirtualButton = false;
void ResetEncoders()
{
  SensorValue(BaseEncoderLeft) = 0;
  SensorValue(BaseEncoderRight) = 0;
}

void ResetArmEncoders()
{
  SensorValue(ArmEncoderLeft) = 0;
  SensorValue(ArmEncoderRight) = 0;
}

int DistanceToCounts(int distance)//Distance in 1/10 inches!
{
  const int circumference = 12.6;//Large omni this variable can only go to tenths decimal
  int counts = 0;//1 count = 4 degrees, initially set to zero

  counts = (distance * 90) / (circumference);//Set counts for proper distance
  return counts;
}

void DriveMotorStop()
{
  motor[LeftBase] = 0;
  motor[RightBase] = 0;
}

void VacumePickUp()
{
  motor[LeftVacume] = 127;
  motor[RightVacume] = 127;
  wait1Msec(2000);
  motor[LeftVacume] = 0;
  motor[RightVacume] = 0;
}

void VacumeSpitOut()
{
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
    wait1Msec(10000);
    motor[LeftVacume] = 0;
    motor[RightVacume] = 0;
}

void MoveStraight(int distance)
{

   int counts;//This is a variable to hold distance in counts

  ResetEncoders();//Reset encoders with function, start at zero!

  counts = DistanceToCounts(distance);

  while (SensorValue(BaseEncoderRight)<counts)
  {

    if (SensorValue(BaseEncoderLeft)>SensorValue(BaseEncoderRight))//Left faster
    {
      motor[LeftBase]=93/2;
      motor[RightBase]=103/2;
    }
    if (SensorValue(BaseEncoderLeft)<SensorValue(BaseEncoderRight))//Right faster
    {
      motor[LeftBase]=103/2;
      motor[RightBase]=93/2;
    }
    if (SensorValue(BaseEncoderLeft)==SensorValue(BaseEncoderRight))//Equal
    {
      motor[LeftBase]=103/2;
      motor[RightBase]=103/2;
    }
  }
  DriveMotorStop();
}


/*void MoveLift(int wantedposition)
{
  ResetArmEncoders();
  if(SensorValue(ArmEncoderLeft)<wantedposition)
  {
    while((SensorValue(ArmEncoderLeft)<wantedposition))
    {
      motor[LeftArm]=127;
      motor[RightArm]=127;
    }
    motor[LeftArm]=0;
    motor[RightArm]=0;
    return;
  }

  if(SensorValue(ArmEncoderLeft)>wantedposition && SensorValue(LimitArmOne) == 0)
  {
    while((SensorValue(ArmEncoderLeft)>wantedposition) && SensorValue(LimitArmOne) == 0)
    {
      motor[LeftArm]=-127;
      motor[RightArm]=-127;
    }
    motor[LeftArm]=0;
    motor[RightArm]=0;
    return;
  }
  return;
}

void MoveLiftDown()
{
  VirtualButton = true;
  while(VirtualButton == true)
  {
    if(SensorValue[LimitArmOne] == 0)
    {
      motor[LeftArm] = -127;
    }
    if(SensorValue[LimitArmTwo] == 0)
    {
      motor[RightArm] = -127;
    }
    if(SensorValue[LimitArmOne] == 1)
    {
      motor[LeftArm] = 0;
    }
    if(SensorValue[LimitArmTwo] == 1)
    {
      motor[RightArm] = 0;
    }
    if(SensorValue[LimitArmOne] == 1 && SensorValue[LimitArmTwo] == 1)
    {
      VirtualButton = false;
    }
  }
}
*/
void TurnRobot (int turnangle)
{
  int wheelseperation = 12;   //Width of robot base
  int rotationdistance;       //Distance wheel needs to turn in 1/10 inches
  int wheelcounts;            //Counts wheel needs to travel
  int absturnangle;           //Absalute value of turnangle
  bool negative = false;      //If a number is negative, this variable gets notated "true."

  absturnangle = 0;

  if(turnangle<0)//If turnangle variable is negative...
  {
    negative = true;//...Then notate it throught variables.
  }

  absturnangle = abs(turnangle);//Compute absolute value (-90 becomes 90 if negative or 90 stays 90)

  rotationdistance = (wheelseperation * absturnangle * 3.14159265) / 36;  //Compute distance in 1/10"; PI = 3.14159265 here
  wheelcounts = DistanceToCounts(rotationdistance);  //Turn distance into counts
  bMotorFlippedMode[port3] = true;  //No reflection

  ResetEncoders();  //Reset encoders for use.

  if(negative == false)
  {
    while(SensorValue(BaseEncoderLeft)<wheelcounts)  //Runs when needed.
    {
      motor[LeftBase] = -63;
      motor[RightBase] = 63;
    }
  }
  else
  {
    while(SensorValue(BaseEncoderLeft)<wheelcounts)
    {
      motor[LeftBase]= 63;
      motor[RightBase]= -63;
    }
  }
  DriveMotorStop();
}

//**Port Mapping**//
//All port 2's are left side
//All port 3's are right side

// End Autonomous Voids

// Start UserControl Voids

void TurntableForward()
{
  if(vexRT[Btn6UXmtr2] == 1)
  {
    motor[LeftTurntable] = 127;
    motor[RightTurntable] = 127;
  }
  else
  {
    motor[LeftTurntable] = 5;
    motor[RightTurntable] = 5;
  }
}

void turnrobot90()
{
  motor[LeftBase] = 127;
  motor[RightBase] = -127;
  wait1msec(505);
  DriveMotorStop();
}

void turnrobotother90red()
{
  motor[LeftBase] = -127;
  motor[RightBase] = 127;
  wait1msec(500);
  DriveMotorStop();
}

void rampmotors()
{
}

void turnrobotother90()
{
  motor[LeftBase] = -127;
  motor[RightBase] = 127;
  wait1msec(700);
  DriveMotorStop();
}

void turntable()
{
  motor[LeftTurntable] = -127;
  motor[RightTurntable] = -127;
  wait1Msec(0);
  motor[LeftTurntable] = 0;
  motor[RightTurntable] = 0;
}

void turntablebegining()
{
  motor[LeftTurntable] = -127;
  motor[RightTurntable] = -127;
  wait1Msec(1000);
  motor[LeftTurntable] = 0;
  motor[RightTurntable] = 0;
}

void TurntableBackward()
{
  if(vexRT[Btn6DXmtr2] == 1)
  {
    motor[LeftTurntable] = -127;
    motor[RightTurntable] = -127;
  }
}
int positiveright = 1;
int positiveleft = 1;
void Base()
{
  if(vexRT(Ch2) > 2)
  {
    positiveright = 1;
  }
  if(vexRT(Ch2) < -2)
  {
    positiveright = -1;
  }
  if(vexRT(Ch2) == 0 || vexRT(Ch2) == 1 || vexRT(Ch2) == -1 || vexRT(Ch2) == 2 || vexRT(Ch2) == 3 ||
    vexRT(Ch2) == 4 || vexRT(Ch2) == -2 || vexRT(Ch2) == -3 || vexRT(Ch2) == -4)
  {
    positiveright = 0;
  }

  if(vexRT(Ch3) > 2)
  {
    positiveleft = 1;
  }
  if(vexRT(Ch3) < -2)
  {
    positiveleft = -1;
  }
  if(vexRT(Ch3) == 0 || vexRT(Ch3) == 1 || vexRT(Ch3) == -1 || vexRT(Ch3) == 2 || vexRT(Ch3) == 3 ||
    vexRT(Ch3) == 4 || vexRT(Ch3) == -2 || vexRT(Ch3) == -3 || vexRT(Ch3) == -4)
  {
    positiveleft = 0;
  }


if(positiveright == 1 && motorpowerright <= 127)
{
  motorpowerright = motorpowerright + 5;
  wait1Msec(10);
}

if(positiveleft == 1 && motorpowerleft <= 127)
{
  motorpowerleft = motorpowerleft + 5;
  wait1Msec(10);
}
if(positiveright == -1 && motorpowerright >= -127)
{
  motorpowerright = motorpowerright - 5;
  wait1Msec(10);
}
if(positiveleft == -1 && motorpowerleft >= -127)
{
  motorpowerleft = motorpowerleft - 5;
  wait1Msec(10);
}
if(positiveright == 0)
{
  motor[RightBase] = 0;
  motorpowerright = 0;
}
if(positiveleft == 0)
{
  motor[LeftBase] = 0;
  motorpowerleft = 0;
}

  motor(LeftBase) = motorpowerleft;
  motor(RightBase) = motorpowerright;
}

void ArmsUp()
{
  if(vexRT[Btn7UXmtr2] == 1 && SensorValue(ArmEncoderLeft) < 2000 && SensorValue(ArmEncoderRight) < 2000) // makes the arms go up if Btn 7 Up is pressed
  {
		if(SensorValue[ArmEncoderRight] == SensorValue[ArmEncoderLeft]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[RightArm] = 127;		    // Right Motor is run at power level 80
			motor[LeftArm]  = 127;		    // Left Motor is run at power level 80
		}
		if(SensorValue[ArmEncoderRight] > SensorValue[ArmEncoderLeft])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[RightArm] = 107;		    // Right Motor is run at power level 60
			motor[LeftArm]  = 127;		    // Left Motor is run at power level 80
		}
		if(SensorValue[ArmEncoderRight] < SensorValue[ArmEncoderLeft])// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[RightArm] = 127;		    // Right Motor is run at power level 80
			motor[LeftArm]  = 107;		    // Left Motor is run at power level 60
		}
  }
  else
  {
    motor[RightArm] = 5;
    motor[LeftArm] = 5;
  }
}

void ArmsDown()
{
  if(vexRT[Btn7DXmtr2] == 1 && SensorValue[ArmEncoderRight] > 0 && SensorValue[ArmEncoderLeft] > 0)  // makes the arms go down if Btn 7 Down is pressed
  {
    if(SensorValue[ArmEncoderRight] == SensorValue[ArmEncoderLeft]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[RightArm] = -127;		    // Right Motor is run at power level 80
			motor[LeftArm]  = -127;		    // Left Motor is run at power level 80
		}
		else if(SensorValue[ArmEncoderRight] > SensorValue[ArmEncoderLeft])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[RightArm] = -107;		    // Right Motor is run at power level 60
			motor[LeftArm]  = -127;		    // Left Motor is run at power level 80
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[RightArm] = -127;		    // Right Motor is run at power level 80
			motor[LeftArm]  = -107;		    // Left Motor is run at power level 60
		}
  }
}

void VacumeUp()
{
  if(vexRT[Btn8UXmtr2] == 1) // makes the vacume go up if Btn 8 up is pressed
  {
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
  }
  else
  {
    motor[LeftVacume] = 0;
    motor[RightVacume] = 0;
  }
}

void VacumeDown()
{
  if(vexRT[Btn8DXmtr2] == 1) // makes the vacume go down if Btn 8 down is pressed
  {
    motor[LeftVacume] = -127;
    motor[RightVacume] = -127;
  }
}

// end UserControl Voids


/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

/* Key for Autonomous Switches
   Switch not pressed Pressed, red iso zone
   Switch Pressed, Blue iso zone
*/
task autonomous()
{
  MoveStraight(Square);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
  while(1) // the code goes forever
  {
    TurntableForward(); // second remote
    TurntableBackward(); // second remote
    Base();
    ArmsUp(); // second remote
    ArmsDown(); // second remote
    VacumeUp(); // second remote
    VacumeDown(); // second remote
  }
}
